 /*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

/**
 *
 * @author Hugo
 */
#ifndef _SPARSEMATRIX_H_
#define _SPARSEMATRIX_H_

#include "vector.h"
#include "matrix.h"
#include <iostream>

// forward declarations
using namespace std;
//----------------------


template <class T>
class SkylineStorage: public Matrix<T>
{
template <class U>    
friend const Matrix<U> operator*(const Matrix<U>&, const SkylineStorage<U>&);
template <class U>    
friend const Vector<U> operator*(const Vector<U>&, const SkylineStorage<U>&);

public:

 SkylineStorage( const Matrix<T>& _M );
 SkylineStorage( const SkylineStorage<T>& _SK );

 // Destructor
 ~SkylineStorage();

 // Solvers
 Vector<T> solve( const Vector<T>&);
 Vector<T> isolve( const Vector<T>& _M );
 Vector<T> PCGsolve( const Vector<T>& _V, double error = 0.0001);
 
 

 // Utility methods
 void print() const;
 Matrix<T> toMatrix();
 void chol();
 void ichol();
 
 
 
 //Calculation operators:
 
 Matrix<T> operator*(const Matrix<T>&) const;	
 Vector<T> operator*(const Vector<T>&) const;
 Matrix<T> operator+(const Matrix<T>&) const;
 Matrix<T> operator-(const Matrix<T>&) const;
 Matrix<T> operator * (const T& a) const;
 //TODO: product between two skylines!
 
 //Equality Operators:
    bool operator == (const Matrix<T>& ) const;
    bool operator != (const Matrix<T>& ) const;
    

private:
 
 int m_dim;
 Vector<T> A;
 IVector P;
 bool isFactorized;
 Matrix<T> pre_multiply(const Matrix<T>&) const;
 Matrix<T> post_multiply(const Matrix<T>&) const;
 Vector<T> pre_multiply(const Vector<T>&) const;
 Vector<T> post_multiply(const Vector<T>&) const;
 
};



//-----------------------------------------------------
template <class T> inline
SkylineStorage<T>::SkylineStorage( const Matrix<T>& _M )
:Matrix<T>(_M), isFactorized(false)
{
 m_dim = _M.getCols();
 int non_zeros = m_dim*(m_dim+1)/2;
    
    //total of non_zeros elements:
    for(int j=0; j<m_dim; j++){
        for(int i=0; i<j; i++){
            if(_M(i,j) == 0){
                non_zeros--;
            }
            else{
                break;
            }
        }
    }
 
 
 
 P.resize(m_dim);
 A.resize(non_zeros);
 int val_index = -1;
 int ptr_index = -1;
 for(int j=0; j<m_dim; j++ ){
  for( int i=0; i<j; i++){
   if(_M(i,j) != 0){
    for(int aux=i; aux<j; aux++){
     A(++val_index) = _M(aux,j);
     }
	break;   
   }	   
  }
   A(++val_index) = _M(j,j);
   P(++ptr_index) = val_index;
  }
}
//-----------------------------------------------------


//-----------------------------------------------------
template <class T> inline
SkylineStorage<T>::SkylineStorage( const SkylineStorage<T>& _SK ):
m_dim(_SK.m_dim),
A(_SK.A),
P(_SK.P),
isFactorized(_SK.isFactorized)
{
    
}
//-----------------------------------------------------


//-----------------------------------------------------
template <class T> inline
SkylineStorage<T>::~SkylineStorage()
{

}
//-----------------------------------------------------

//-----------------------------------------------------
template <class T> inline
Vector<T> SkylineStorage<T>::solve( const Vector<T>& _V )
{
 if(!isFactorized)
     this->chol();
 Vector<T> r(_V);
 double sum = 0.;
  int hi=0;
  int I=0;
  for(int i=1; i<m_dim; i++){
   sum = 0.;
   hi = P(i-1) - P(i) + i + 1;
   I = P(i) - i;
   for(int j=hi; j<i; j++){
    sum = sum + A(I+j)*r(j);
   }
   r(i) = r(i) - sum;
  }

  for(int i=0; i<m_dim; i++){
   r(i) = r(i)/A(P(i));
  }

  int J = 0;
  int hj = 0;
  for(int j=m_dim-1; j>0; j--){
   J = P(j) - j;
   hj = P(j-1) - P(j) + j + 1;
   for(int i=hj; i<j; i++){
    r(i) = r(i) - r(j)*A(J + i);
   }
  }
  
  return r;
}
//-----------------------------------------------------
template <class T> inline
Vector<T> SkylineStorage<T>::PCGsolve( const Vector<T>& _V, double e)
{
    SkylineStorage<T> A(*this);
    Vector<T> x(m_dim);
    Vector<T> r(m_dim);
    r = _V;
    Vector<T> d(m_dim);
    d = this->isolve(r);
    Vector<T> aux(m_dim);
    Vector<T> dot_aux(m_dim);
    double dot_new = r*d;
    double dot_old;
    double alpha;
    double beta;
    int i = 1;
    double error = fabs(e*e*dot_new);
    
    
    while(fabs(dot_new) > error & i < 10000){
        dot_aux = A*d;
        alpha = dot_new/(d*dot_aux);
        x = x + d*alpha;
        if(i%50 == 0){
        r = _V - A*x;
        }
        else{
        r = r - (dot_aux*alpha);
        }
        aux = this->isolve(r);
        dot_old = dot_new;
        dot_new = r*aux;
        beta = dot_new/dot_old;
        d = aux + (d*beta);
        i++;
    }
    return x;
}
//-----------------------------------------------------
//-----------------------------------------------------
template <class T> inline
Vector<T> SkylineStorage<T>::isolve( const Vector<T>& _V )
{
 if(!isFactorized)
     this->ichol();
 Vector<T> r(_V);
 double sum = 0.;
  int hi=0;
  int I=0;
  for(int i=1; i<m_dim; i++){
   sum = 0.;
   hi = P(i-1) - P(i) + i + 1;
   I = P(i) - i;
   for(int j=hi; j<i; j++){
    sum = sum + A(I+j)*r(j);
   }
   r(i) = r(i) - sum;
  }

  for(int i=0; i<m_dim; i++){
   r(i) = r(i)/A(P(i));
  }

  int J = 0;
  int hj = 0;
  for(int j=m_dim-1; j>0; j--){
   J = P(j) - j;
   hj = P(j-1) - P(j) + j + 1;
   for(int i=hj; i<j; i++){
    r(i) = r(i) - r(j)*A(J + i);
   }
  }
  
  return r;
}
//-----------------------------------------------------

//-----------------------------------------------------
template <class T> inline
void SkylineStorage<T>::print() const
{
 int j;
 cout << "\nA = \n";
 for( j=0; j < A.getLength(); j++ )
  cout << A(j) << '\n';
 cout << "\nP = \n";
 for( j=0; j < m_dim; j++ )
  cout << P(j) << '\n';
 cout << endl;

}
//-----------------------------------------------------

//-----------------------------------------------------
template <class T> inline
void SkylineStorage<T>::chol()
{
 int hi;
 int hj;
 int J = 0;
 int I = 0;

 for(int j=1; j < m_dim; j++){
  J = P(j) - j;
  hj = P(j-1) - P(j) + j + 1;
  // Reduce the off-diagonal elements;
  
  for(int i = hj; i < j ; i++){
  if(i!=0)
     hi = P(i-1) - P(i) + i + 1;
  else
     hi=0;
     T sum = 0;
   int maxh = max(hi, hj);
   for(int m=maxh; m < i; m++ ){
    I = P(i) - i;
    sum +=  A(I+m)*A(J+m); 
   }
   A(J + i) = A(J + i) - sum;
  }

  //Reduce the diagonal elements

  T sum=0;
  for(int m=hj; m<j; m++){
   sum +=  A(J+m)*A(J+m)/A(P(m));
  }
  A(P(j)) -=  sum;


  //Complete reduce:

  for(int m=hj; m < j; m++){
   A(J+m) = A(J+m)/A(P(m)); 
  }
 }
isFactorized = true;
}
//-----------------------------------------------------

//-----------------------------------------------------
template <class T> inline
void SkylineStorage<T>::ichol()
{
 int hi;
 int hj;
 int J = 0;
 int I = 0;

 for(int j=1; j < m_dim; j++){
  J = P(j) - j;
  hj = P(j-1) - P(j) + j + 1;
  // Reduce the off-diagonal elements;
  
  for(int i = hj; i < j ; i++){
  if(A(J+i) != 0){       
  if(i!=0)
     hi = P(i-1) - P(i) + i + 1;
  else
     hi=0;
   double sum = 0;
   int maxh = max(hi, hj);
   for(int m=maxh; m < i; m++ ){
    I = P(i) - i;
    sum +=  A(I+m)*A(J+m); 
   }
   A(J + i) = A(J + i) - sum;
  }
  }

  //Reduce the diagonal elements

  double sum=0;
  for(int m=hj; m<j; m++){
   sum +=  A(J+m)*A(J+m)/A(P(m));
  }
  A(P(j)) -=  sum;


  //Complete reduce:

  for(int m=hj; m < j; m++){
   A(J+m) = A(J+m)/A(P(m)); 
  }
 }
isFactorized = true;
}
//-----------------------------------------------------

//-----------------------------------------------------
template <class T> inline
Matrix<T> SkylineStorage<T>::pre_multiply(const Matrix<T>& _M) const{
Matrix<T> result_matrix(m_dim,_M.getCols());
result_matrix.zero();
int hi;
int I;
for(int c = 0; c < _M.getCols(); c++ ){
	result_matrix(0,c) = A(0)*_M(0,c);
}
for(int i=1; i<m_dim; i++){
	hi = P(i-1) - P(i) + i + 1;
	I = P(i) - i;
	for(int k=0; k<_M.getCols(); k++){
		for(int j=hi; j<i; j++){
			result_matrix(i,k) += A(I+j)*_M(k,j);
			result_matrix(j,k) += A(I+j)*_M(k,i);
		}
		result_matrix(i,k) += A(P(i))*_M(k,i);
	}
}
return result_matrix;
}
//-----------------------------------------------------

//-----------------------------------------------------
template <class T> inline
Matrix<T> SkylineStorage<T>::post_multiply(const Matrix<T>& _M) const{
	Matrix<T> result_matrix(_M.getRows(),m_dim);
	result_matrix.zero();
	int hj;
	int J;
	for(int r = 0; r < _M.getRows(); r++ ){
		result_matrix(r,0) = A(0)*_M(r,0);
	}
	for(int j=1; j<m_dim; j++){
		hj = P(j-1) - P(j) + j + 1;
		J = P(j) - j;
		for(int k=0; k<_M.getRows(); k++){
			for(int i=hj; i<=j-1; i++){
				result_matrix(k,i) += A(J+i)*_M(k,j);
				result_matrix(k,j) += A(J+i)*_M(k,i);
			}
			result_matrix(k,j) += A(P(j))*_M(k,j);
		}
	}
	return result_matrix;
}
//-----------------------------------------------------

//-----------------------------------------------------
template <class T> inline
Vector<T> SkylineStorage<T>::pre_multiply(const Vector<T>& _V) const{
Vector<T> result_vector(m_dim);
result_vector.zero();
int hi;
int I;
result_vector(0) = A(0)*_V(0);
for(int i=1; i<m_dim; i++){
	hi = P(i-1) - P(i) + i + 1;
	I = P(i) - i;
		for(int j=hi; j<=i-1; j++){
			result_vector(i) += A(I+j)*_V(j);
			result_vector(j) += A(I+j)*_V(i);
		}
		result_vector(i) += A(P(i))*_V(i);
	}
return result_vector;
}
//-----------------------------------------------------

//-----------------------------------------------------
template <class T> inline
Vector<T> SkylineStorage<T>::post_multiply(const Vector<T>& _V) const{
Vector<T> result_vector(m_dim);
result_vector.zero();
int hi;
int I;
result_vector(0) = A(0)*_V(0);

for(int i=1; i<m_dim; i++){
	hi = P(i-1) - P(i) + i + 1;
	I = P(i) - i;
		for(int j=hi; j<=i-1; j++){
			result_vector(i) += A(I+j)*_V(j);
			result_vector(j) += A(I+j)*_V(i);
		}
		result_vector(i) += A(P(i))*_V(i);
	}
return result_vector;
}
//-----------------------------------------------------


//-----------------------------------------------------
template <class T> inline
Matrix<T> SkylineStorage<T>::operator*(const Matrix<T>& _M) const{
	return this->pre_multiply(_M);
}
//-----------------------------------------------------


//-----------------------------------------------------
template <class T> inline
Vector<T> SkylineStorage<T>::operator*(const Vector<T>& _V) const{
	return this->pre_multiply(_V);
}
//-----------------------------------------------------
template <class T> inline
Matrix<T> SkylineStorage<T>::operator+(const Matrix<T>& _M) const{
    Matrix<T> result_matrix(m_dim,m_dim);
    result_matrix.zero();
    int hi;
    int I;

result_matrix(0,0) =  A(0) + _M(0,0);   
for(int i=1; i<m_dim; i++){
	hi = P(i-1) - P(i) + i + 1;
	I = P(i) - i;
                        for(int j=hi; j<i; j++){
                             result_matrix(i,j) = A(I+j) + _M(i,j);    
                             result_matrix(j,i) = A(I+j) + _M(j,i);    
                        }
                        result_matrix(i,i) = A(P(i)) + _M(i,i);
	}
return result_matrix;
}
//-----------------------------------------------------
template <class T> inline
Matrix<T> SkylineStorage<T>::operator-(const Matrix<T>& _M) const{
    Matrix<T> result_matrix(m_dim,m_dim);
    result_matrix.zero();
    int hi;
    int I;

result_matrix(0,0) =  A(0) - _M(0,0);   
for(int i=1; i<m_dim; i++){
	hi = P(i-1) - P(i) + i + 1;
	I = P(i) - i;
                        for(int j=hi; j<i; j++){
                             result_matrix(i,j) = A(I+j) - _M(i,j);    
                             result_matrix(j,i) = A(I+j) - _M(j,i);    
                        }
                        result_matrix(i,i) = A(P(i)) - _M(i,i);
	}
return result_matrix;
}
//-----------------------------------------------------
template <class T> inline
Matrix<T> SkylineStorage<T>::operator*(const T& _c) const{

Matrix<T> result_matrix = *this;
int hi;
int I;

result_matrix(0,0) *=_c;   
for(int i=1; i<m_dim; i++){
    hi = P(i-1) - P(i) + i + 1;
    I = P(i) - i;
        for(int j=hi; j<i; j++){
             result_matrix(i,j) *=_c;    
             result_matrix(j,i) *=_c;    
        }
        result_matrix(i,i) *= _c;
    }
return result_matrix;
}
//-----------------------------------------------------

//-----------------------------------------------------
template <class T> inline
bool SkylineStorage<T>::operator==(const Matrix<T>& _M) const{

int hi;
int I;

if(_M(0,0)!=A(0))
    return false;
for(int i=1; i<m_dim; i++){
    hi = P(i-1) - P(i) + i + 1;
    I = P(i) - i;
        for(int j=hi; j<i; j++){
             if(A(I+j) != _M(i,j) || A(I+j) != _M(i,j)){
                 return false;
        }
        if(A(P(i)) != _M(i,i)){
            return false;
        }
    }
}
return true;
}
//-----------------------------------------------------

//-----------------------------------------------------
template <class T> inline
bool SkylineStorage<T>::operator!=(const Matrix<T>& _M) const{
    return !this->operator ==(_M);
}
//-----------------------------------------------------



//-----------------------------------------------------
template <class U>
const Vector<U> operator*(const Vector<U>& _V, const SkylineStorage<U>& _SM){
	return _SM.post_multiply(_V);
}



//-----------------------------------------------------


//-----------------------------------------------------
template <class U> inline
const Matrix<U> operator*(const Matrix<U>& _M, const SkylineStorage<U>& _SM){
	return _SM.post_multiply(_M);
}
//-----------------------------------------------------
//</editor-fold>
//-----------------------------------------------------
template <class T> inline
Matrix<T> SkylineStorage<T>::toMatrix(){
    Matrix<T> matrix(m_dim,m_dim);
	matrix.zero();
    int J;
    int hj;
    matrix(0,0) = A(0);
    for(int j =1; j<m_dim; j++){
        J = P(j) - j;
        hj = P(j-1) - P(j) + j + 1;
        for(int i=hj; i<j; i++){
            matrix(i,j) = A(J+i);
            matrix(j,i) = A(J+i);
        }
       matrix(j,j) = A(P(j));
    }
    return matrix;
}
//-----------------------------------------------------

typedef SkylineStorage<double> DSkylineStorage;
typedef SkylineStorage<int> ISkylineStorage;
#endif // _SPARSEMATRIX_H_